#include "DataManagerThread.h"

xTimerHandle DMtimer;								 
static void DMtimerCallback( xTimerHandle pxTimer );

xQueueHandle DataManagerThreadQueue;
extern 	xQueueHandle UARTQueue;
extern xQueueHandle CollisionThreadQueue;
extern xQueueHandle CommandThreadQueue;
extern xQueueHandle MappingThreadQueue;
extern xQueueHandle SpeedThreadQueue;
extern xQueueHandle WebQueue;
extern unsigned int checkStart;
Comms_Msg_Struct *Msg;


Msg_Type_Enum msgTpye;

int prevTotalValue;
int curTotalValue;

Comms_Msg_Struct *CurUltra;
Comms_Msg_Struct *CurColor;
Comms_Msg_Struct *CurEncod;
Comms_Msg_Struct *CurEncodTotal;
int runTotal[10];
int curRun;

unsigned char *lcdMsg;
unsigned char buff[4];
unsigned char rData;
uint8_t counter = 0;

// (Ticks * instruction cycle * post-scale * pre-scale * microseconds) / clockSpeed
//const uint8_t timerPeriod = (200 * 4 * 10 * 1 * 1000000) / 32000000;

void print_hex_packet(uint8_t *packet,uint16_t size,unsigned char *Buffer)
{
    const char *hex_Map = "0123456789ABCDEF";        
	int i,j;
	j=0;
	for (i=0;i<size;i++)
	{
		Buffer[j++] = hex_Map[ packet[i] >>    4 ];
		Buffer[j++] = hex_Map[ packet[i] &  0x0f ];
	}
	Buffer[j++] = 0;
}

void DataManagerThreadTaskCode( void * pvParameters )
{
// // Initialize LCD
//GLCD_Init();
//GLCD_Clear(Green);
//GLCD_SetTextColor(Green);
//GLCD_SetBackColor(Blue);
//GLCD_Clear(Blue);
///////////////////////////
	Comms_Msg_Struct cms1;
	Comms_Msg_Struct cms2;
	Comms_Msg_Struct cms3;
	Comms_Msg_Struct cms4;
	CurUltra = &cms1;
	CurColor = &cms2;
	CurEncod = &cms3;
	CurEncodTotal = &cms4;

	int i;

	CurUltra->msgType = 0x05;
	CurUltra->counter = 0x00;
	CurUltra->size = 0x06;
	for(i = 0; i < 6; i++)
		CurUltra->data[i] = 0;
	
	CurColor->msgType = 0x05;
	CurColor->counter = 0x00;
	CurColor->size = 0x01;
	for(i = 0; i < 6; i++)
		CurColor->data[i] = 0;

	CurEncod->msgType = 0x05;
	CurEncod->counter = 0x00;
	CurEncod->size = 0x02; 
	for(i = 0; i < 6; i++)
		CurEncod->data[i] = 0;

	CurEncodTotal->msgType = 0x05;
	CurEncodTotal->counter = 0x00;
	CurEncodTotal->size = 0x04; 
	for(i = 0; i < 6; i++)
		CurEncodTotal->data[i] = 0;

	curRun = 0;
	for(i = 0; i < 10; i++)
		runTotal[i] = 0;

	DMtimer = xTimerCreate
	(  /* Just a text name, not used by the RTOS kernel. */
	 (const signed char*)"DMtimer",
	 /* The timer period in ticks. */
	 ( 1500 / portTICK_RATE_MS  ),
	 /* The timers will auto-reload themselves when they expire. */
	 pdTRUE,
	 /* Assign each timer a unique id equal to its array index. */
	 ( void * ) 4,
	 /* Each timer calls the same callback when it expires. */
	 DMtimerCallback
	);

	if( xTimerStart( DMtimer, 0 ) != pdPASS )
	{
	 /* The timer could not be set into the Active state. */				 
	}

    unsigned char buf[20];
	curTotalValue = 0;
 	prevTotalValue = 0;
	for(;;)
	{
	 	if( DataManagerThreadQueue != 0 )
    	{
			if( xQueueReceive( DataManagerThreadQueue,(void*) &( Msg ), ( portTickType ) 0 ) )
		    {
				if(Msg->msgType == ULTRASONIC)
				{
					if((Msg->counter - CurUltra->counter) > 10)
					{
//						if( CommandThreadQueue != 0 )
//						{
//							if(xQueueSend( CommandThreadQueue, (void * ) &CurUltra, ( portTickType ) 0 ) != pdPASS){};
//						}
					}

					CurUltra->counter = Msg->counter;					
					int L_0, L_1, R_0, R_1;
					L_0 = Msg->data[0];
					L_1 = Msg->data[1];		
					R_1 = Msg->data[2];	
					R_0 = Msg->data[3];	
					
					L_0 = 15 - L_0;
					L_1 = 15 - L_1;		
					R_1 = 15 - R_1;	
					R_0 = 15 - R_0;	



//					RIR_0 = 13 - RIR_0;
//					RIR_1 = 13 - RIR_1;
//					LIR_0 = 13 - LIR_0;
//					LIR_1 = 13 - LIR_1;
//					LIR_2 = 255 - LIR_2;
//					RIR_2 = 255 - RIR_2;
					//LIR = 12 - LIR;
					//RIR = 12 - RIR;
					//CIR = 12 - CIR;

					CurUltra->data[0] = (unsigned char)L_0;
					CurUltra->data[1] = (unsigned char)L_1;
					CurUltra->data[2] = (unsigned char)R_1;
					CurUltra->data[3] = (unsigned char)R_0;

//					sprintf(buf,"L0: %d",L_0);
//				    GLCD_ClearLn(2,1);
//				    GLCD_DisplayString(2,0,1,buf);
//					sprintf(buf,"R0: %d",R_0);
//				    GLCD_ClearLn(3,1);
//				    GLCD_DisplayString(3,0,1,buf);
//
//					sprintf(buf,"L1: %d",L_1);
//				    GLCD_ClearLn(4,1);
//				    GLCD_DisplayString(4,0,1,buf);
//					sprintf(buf,"R1: %d",R_1);
//				    GLCD_ClearLn(5,1);
//				    GLCD_DisplayString(5,0,1,buf);
//					
//					sprintf(buf,"L2: %d",L_2);
//				    GLCD_ClearLn(6,1);
//				    GLCD_DisplayString(6,0,1,buf);
//					sprintf(buf,"R2: %d",R_2);
//				    GLCD_ClearLn(7,1);
//				    GLCD_DisplayString(7,0,1,buf);
					
					if( CollisionThreadQueue != 0 )
					{
						if(xQueueSend( CollisionThreadQueue, (void * ) &CurUltra, ( portTickType ) 0 ) != pdPASS){};
					}
				}
				else if(Msg->msgType == COLOR)
				{
					if((Msg->counter - CurColor->counter) > 10)
					{
//						if( CommandThreadQueue != 0 )
//						{
//							if(xQueueSend( CommandThreadQueue, (void * ) &CurColor, ( portTickType ) 0 ) != pdPASS){};
//						}
					}

					CurColor->counter = Msg->counter;

//
					int rAdj = Msg->data[1];//*2;
					rData = Msg->data[1];
					int gAdj = Msg->data[3];
					int bAdj = Msg->data[5];//*3;
//					sprintf(buf,"R: %d",rAdj);
//				    GLCD_ClearLn(6,1);
//				    GLCD_DisplayString(6,0,1,buf);
//					sprintf(buf,"G: %d",gAdj);
//					GLCD_ClearLn(7,1);
//				    GLCD_DisplayString(7,0,1,buf);
//					sprintf(buf,"B: %d",bAdj);
//					GLCD_ClearLn(8,1);
//				    GLCD_DisplayString(8,0,1,buf);
////
//					///////////////////////////////////////////////////////////////////////
//					//FOR FINISH LINE DETECT SAVE RUN TOTAL DISTANCE AND INCREMENT CURRUN
//					///////////////////////////////////////////////////////////////////////

//					if( (rAdj >= 250 && rAdj < 300) && (bAdj >= 250 && bAdj < 270) )  	//Light Pink Detected
//					{
//						CurColor->data[0] = Low;
//					}
//					else if(rAdj < 50 && bAdj < 50)	//Black Detected
//					{
//						CurColor->data[0] = High;
//					}
//					else if(rAdj > 375) 	//Hot Pink Detected
//					{
//						CurColor->data[0] = Finish;
//					}
//					else								//No Color (Floor) Dtected
//					{
//						CurColor->data[0] = None;
//					}
					CurColor->data[0] = rData;
					if( SpeedThreadQueue != 0 )
					{	
						if(xQueueSend( SpeedThreadQueue, (void * ) &CurColor, ( portTickType ) 0 ) != pdPASS){};	
					}				
				}
				else if(Msg->msgType == ENCODER)
				{					 
//					if((Msg->counter - CurEncod->counter) > 10)
//					{
//						if( CommandThreadQueue != 0 )
//						{
//							if(xQueueSend( CommandThreadQueue, (void * ) &CurEncod, ( portTickType ) 0 ) != pdPASS){};
//						}
//					}

					CurEncod->counter = Msg->counter;

					curTotalValue = Msg->data[0];
					curTotalValue = (curTotalValue<<8) + Msg->data[1];
//
					//sprintf(buf,"Distance: %d",curTotalValue);
				    //GLCD_ClearLn(2,1);
				    //GLCD_DisplayString(2,0,1,buf);
					int i;
					for(i = 0; i < 10; i++)
						curTotalValue -= runTotal[i];

					//prevTotalValue = CurEncodTotal->data[2];
					//prevTotalValue = (prevTotalValue<<8) + CurEncodTotal->data[3];

					//int relative = curTotalValue - prevTotalValue;

					//prevTotalValue = curTotalValue;

					//	sprintf(buf,"RelDist: %d",relative);
					//GLCD_ClearLn(3,1);
				    //GLCD_DisplayString(3,0,1,buf);
					CurEncodTotal->data[2] = (unsigned char)((curTotalValue>>8) && 0xFF);
					CurEncodTotal->data[3] = (unsigned char)(curTotalValue && 0xFF);

//					CurEncod->data[0] = (unsigned char) ((relative>>8) & 0xFF);
//					CurEncod->data[1] = (unsigned char) (relative & 0xFF);
//
//					if( SpeedThreadQueue != 0 )
//					{
//						if(xQueueSend( SpeedThreadQueue, (void * ) &CurEncod, ( portTickType ) 0 ) != pdPASS){};
//					}
					if( MappingThreadQueue != 0 )
					{
						if(xQueueSend( MappingThreadQueue, (void * ) &CurEncodTotal, ( portTickType ) 0 ) != pdPASS){};
					}
				}
				else if( Msg->msgType == Speed)
				{
					runTotal[curRun] = curTotalValue;
					curRun++;
				}
			}
		}
	}
}


void Init_DataManagerThread()
{
	xTaskHandle DataManagerThreadHandle;	
	/* Create the task, storing the handle.  Note that the passed parameter 
	ucParameterToPass must exist for the lifetime of the task, so in this 
	case is declared static.  If it was just an an automatic stack variable 
	it might no longer exist, or at least have been corrupted, by the time
	the new task attempts to access it. */
	xTaskCreate( DataManagerThreadTaskCode, ( signed char * ) "DataManager Thread Task", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY
	, &DataManagerThreadHandle );
	DataManagerThreadQueue = xQueueCreate( 10, sizeof(Comms_Msg_Struct *)  );
}

static void DMtimerCallback( xTimerHandle pxTimer )
{	    
	int relative = curTotalValue - prevTotalValue;

	prevTotalValue = curTotalValue;

	CurEncod->data[0] = (unsigned char) ((relative>>8) & 0xFF);
	CurEncod->data[1] = (unsigned char) (relative & 0xFF);
	CurEncod->data[2] = (unsigned char) ((curTotalValue>>8) & 0xFF);
	CurEncod->data[3] = (unsigned char) (curTotalValue & 0xFF);

	if( SpeedThreadQueue != 0 )
	{
		if(xQueueSend( SpeedThreadQueue, (void * ) &CurEncod, ( portTickType ) 0 ) != pdPASS){};
	}
}